using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;

namespace XnaUtil
{
    /// <summary>
    /// Functions that help with the use of Euler angles.
    /// All angles are in radians.
    /// </summary>
    public class EulerHelper
    {
        /// <summary>
        /// Converts an Euler rotation in the order XYZ to a
        /// quaternion.
        /// </summary>
        /// <param name="xyz">Rotation angles in XYX order.</param>
        /// <returns>Quaternion rotation value.</returns>
        public static Quaternion XyzToQuaternion(Vector3 xyz)
        {
            float xcos = (float)Math.Cos(xyz.X/2);
            float xsin = (float)Math.Sin(xyz.X/2);
            float ycos = (float)Math.Cos(xyz.Y/2);
            float ysin = (float)Math.Sin(xyz.Y/2);
            float zcos = (float)Math.Cos(xyz.Z/2);
            float zsin = (float)Math.Sin(xyz.Z/2);

            return new Quaternion(
                xsin * ycos * zcos - xcos * ysin * zsin,
                xcos * ysin * zcos + xsin * ycos * zsin,
                xcos * ycos * zsin - xsin * ysin * zcos,
                xcos * ycos * zcos + xsin * ysin * zsin);
        }

        /// <summary>
        /// Converts an Euler rotation in the order YXZ to a
        /// quaternion.
        /// </summary>
        /// <param name="xyz">Rotation angles in YXZ order.</param>
        /// <returns>Quaternion rotation value.</returns>
        public static Quaternion YxzToQuaternion(Vector3 xyz)
        {
            float xcos = (float)Math.Cos(xyz.X / 2);
            float xsin = (float)Math.Sin(xyz.X / 2);
            float ycos = (float)Math.Cos(xyz.Y / 2);
            float ysin = (float)Math.Sin(xyz.Y / 2);
            float zcos = (float)Math.Cos(xyz.Z / 2);
            float zsin = (float)Math.Sin(xyz.Z / 2);

            return new Quaternion(
                ycos * xsin * zcos - ysin * xcos * zsin,
                ysin * xcos * zcos + ycos * xsin * zsin,
                ycos * xcos * zsin + ysin * xsin * zcos,
                ycos * xcos * zcos - ysin * xsin * zsin);
        }



        /// <summary>
        /// Converts a Quaternion to an Euler angle rotation
        /// in the order XYZ.
        /// </summary>
        /// <param name="q">Quaternion rotation to convert.</param>
        /// <returns>Vector3 containing the XYZ rotation angles.</returns>
        public static Vector3 QuaternionToXyz(Quaternion q)
        {
            double a2 = 2 * (q.W * q.Y - q.X * q.Z);
            float x, y;
            if (a2 <= -0.99999)
            {
                x = 2 * (float)Math.Atan2(q.X, q.W);
                y = -(float)Math.PI / 2;
                return new Vector3(
                    x,
                    y,
                    0);
            }
            else if (a2 >= 0.99999)
            {
                x = 2 * (float)Math.Atan2(q.X, q.W);
                y = (float)Math.PI / 2;
                return new Vector3(
                    x,
                    y,
                    0);
            }
            else
                y = (float)Math.Asin(a2);

            return new Vector3(
                (float)Math.Atan2(2 * (q.W * q.X + q.Y * q.Z), (1 - 2 * (q.X * q.X + q.Y * q.Y))),
                y,
                (float)Math.Atan2(2 * (q.W * q.Z + q.X * q.Y), (1 - 2 * (q.Y * q.Y + q.Z * q.Z))));
        }



        /// <summary>
        /// Converts a Quaternion to an Euler angle rotation
        /// in the order YXZ.
        /// </summary>
        /// <param name="q">Quaternion rotation to convert.</param>
        /// <returns>Vector3 containing the YXZ rotation angles.</returns>
        public static Vector3 QuaternionToYxz(Quaternion q)
        {
            double a2 = 2 * (q.W * q.X + q.Y * q.Z);
            float x, y;
            if (a2 <= -0.99999)
            {
                y = 2 * (float)Math.Atan2(q.Y, q.W);
                x = -(float)Math.PI / 2;
                return new Vector3(
                    x,
                    y,
                    0);
            }
            else if (a2 >= 0.99999)
            {
                y = 2 * (float)Math.Atan2(q.Y, q.W);
                x = (float)Math.PI / 2;
                return new Vector3(
                    x,
                    y,
                    0);
            }
            else
                x = (float)Math.Asin(a2);

            return new Vector3(
                x,
                (float)Math.Atan2(2 * (q.W * q.Y - q.X * q.Z), (1 - 2 * (q.Y * q.Y + q.X * q.X))),
                (float)Math.Atan2(2 * (q.W * q.Z - q.Y * q.X), (1 - 2 * (q.X * q.X + q.Z * q.Z))));
        }
    }
}
